/*NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN

	Copyright (c) 2009 Jounayd Id Salah.
	All rights reserved.

	This file is part of Blacklib.

	Blacklib is free software: you can redistribute it and/or modify it under the 
	terms of the GNU Lesser General Public License as published by the Free Software 
	Foundation, either version 3 of the License, or (at your option) any later version.
	Blacklib is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; 
	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
	PURPOSE.  See the GNU Lesser General Public License for more details.
	You should have received a copy of the GNU Lesser General Public License along with 
	Blacklib.  If not, see <http://www.gnu.org/licenses/>.
*/
//NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN

#include "maths/func.h"

namespace black
{

	//__________________________________________________________________________
	float cos( const float x ) { return ::cos( x ); }

	//__________________________________________________________________________
	float sin( const float x ) { return ::sin( x ); }

	//__________________________________________________________________________
	void sinCos( float& fSin, float& fCos, const float fAngle )
	{
#ifdef bSSE
		const unsigned long uiSign = (unsigned long)( fAngle * F_INV_PI ) << 31;
		data32 uSin;
		fCos = cos( fAngle );
		uSin.f = sqrt( 1.f - fCos*fCos );
		uSin.i ^= uiSign;
		fSin = uSin.f;
#else
		const unsigned long uiSign = (unsigned long)( fAngle * F_INV_PI ) & 0x00000001;
		fCos = cos( fAngle );
		fSin = sqrt( 1.f - fCos*fCos );
		fSin = uiSign ? -fSin : fSin;
#endif
	}

	//__________________________________________________________________________
	float cosFast( const float x )
	{
		return sinFast( x + F_PI_2 );
	}

	//__________________________________________________________________________
	/** Optimized version of the algorithm of Nick at DevMaster.net.
	 It uses a lerp between a parabola and a squared parabola which gives better results (faster and more accurate) than taylor polynomials.
	 */
	/* Original algorithm from Nick(only in the range of -pi, pi) :
	float sine(float x)
	{
		const float B = 4/pi;
		const float C = -4/(pi*pi);

		float y = B * x + C * x * abs(x);

		#ifdef EXTRA_PRECISION
		//  const float Q = 0.775;
		const float P = 0.225;

		y = P * (y * abs(y) - y) + y;   // Q * y + P * y * abs(y)
		#endif
	}
	*/
	float sinFast( float x )
	{
		// wrap to [-0.5, .5]
		x *= F_INV_PI;
		const int iRound = (int)( x );
		x -= (float)iRound;

#ifdef bSSE
		const unsigned int uiAngleGreaterThanPi = iRound & 0x00000001;
#else
		const unsigned int uiSign = iRound << 31; // sign is defined by the parity of iRound
#endif
		data32 uRes;
		/*if ( x < 0.f )
			fRes = ( 4.f + 4.f * x ) * x;
		else
			fRes = ( 4.f - 4.f * x ) * x;
		fRes = .225f * ( fRes * abs(fRes) - fRes ) + fRes;*/
		if ( x < 0.f ) // branching seems to be faster than an abs when sse intrinsics are activated with visual studio
			uRes.f = x * (1.f + x);
		else
			uRes.f = x * (1.f - x);
		uRes.f = uRes.f * ( 4.f * abs(uRes.f) + 3.f );
#ifdef bSSE
		return uiAngleGreaterThanPi ? -uRes.f : uRes.f;
#else
		uRes.ui ^= uiSign;
		return uRes.f;
#endif
	}

	//__________________________________________________________________________
	void fastSinCos( float& fSin, float& fCos, const float fAngle )
	{
		const unsigned long uiSign = (unsigned long)( fAngle * F_INV_PI ) << 31;
		data32 uSin;
		fCos = cosFast( fAngle );
		uSin.f = sqrt( 1.f - fCos*fCos );
		uSin.i ^= uiSign;
		fSin = uSin.f;
	}

	//__________________________________________________________________________
	float tan ( float x )				{ return ::tanf( x ); }

	//__________________________________________________________________________
	float acos ( float x )
	{
		assert( x >= -1.f && x <= 1.f );
		return ::acosf( x );
		//return sqrt(1-x)*(1.5707963267948966192313216916398f + x*(-0.213300989f + x*(0.077980478f + x*-0.02164095f)));
	}

	//__________________________________________________________________________
	float acosSafe ( float x )
	{
		static const float fMinAllowed = -1.f - fEPSILON;
		static const float fMaxAllowed =  1.f + fEPSILON;

		assert( x >= fMinAllowed && x <= fMaxAllowed );
		x = max( -1.f, x );
		x = min( 1.f, x );
		
		return ::acosf( x );
	}

	//__________________________________________________________________________
	float asin ( float x )
	{
		assert( x >= -1.f && x <= 1.f );
		return ::asinf( x );
	}

	//__________________________________________________________________________
	float asinSafe ( float x )
	{
		static const float fMinAllowed = -1.f - fEPSILON;
		static const float fMaxAllowed =  1.f + fEPSILON;

		assert( x >= fMinAllowed && x <= fMaxAllowed );
		x = max( -1.f, x );
		x = min( 1.f, x );

		return ::asinf( x );
	}

	//__________________________________________________________________________
	float atan ( float x )				{ return ::atanf( x ); }

	//__________________________________________________________________________
	float atan2 ( float x, float y )	{ return ::atan2f( x, y ); }

	//__________________________________________________________________________
	float mod2pi ( const float x )
	{
		return x - (float)( (int)( x * F_INV_2_PI )) * F_2_PI;
	}
}
